
#include "config.h"

// state variables
//boolean turnL = 0;
//boolean turnR = 0;
//boolean alarm = 0;
//boolean music = 0;
//boolean strobe = 0;
int MAXTHROTTE = 2200;
int NEUTHROTTE = 1600;
int MINTHROTTE = 900;

int move = 0;// 2 neutral // 1 break //0reverse 
int moveRES = 0;// 3 for // 2 neutral // 1 break //0reverse 
int backSW = 0;
int mode = 0;
int gasPover = 0;
//boolean fog = 0;
int index1 = 0;
int timeStrob = 0;
unsigned long timeAlarm = 0;
unsigned long timeAlarm2 = 0;
unsigned int index2 = 0;

char strobArr1[] = {255,0,255,0,255,0,255,0,255,0,255,0,255,0,255,0,255,0,255,0};
char alarmArr1[] = {20,50,100,150,200,255,150,100,50,20,0,0,0,0,0,0,0,0,0,0};
int movear[3] = {0,0,0};

 
// rc chanel var 
int valTurn = 0;
int valMove = 0;
int valSw = 0; 

#include <Servo.h> 
#include <SPI.h>

// Clock and data pins are pins from the hardware SPI, you cannot choose them yourself.
// Data pin is MOSI (Arduino: 11, Arduino Mega: 51, Teensy 2.0: 2, Teensy 2.0++: 22) 
// Clock pin is SCK (Arduino: 13, Arduino Mega: 52, Teensy 2.0: 1, Teensy 2.0++: 21)

// You can choose the latch pin yourself.
const int ShiftPWM_latchPin=8;

// If your LED's turn on if the pin is low, set this to 1, otherwise set it to 0.
const bool ShiftPWM_invertOutputs = 0; 

#include <ShiftPWM.h>   // include ShiftPWM.h after setting the pins!

// Here you set the number of brightness levels, the update frequency and the number of shift registers.
// These values affect the load of ShiftPWM.
// Choose them wisely and use the PrintInterruptLoad() function to verify your load.
unsigned char maxBrightness = 255;
unsigned char pwmFrequency = 75;
int numRegisters = 2;
int numOutputs = numRegisters*8;
int numRGBLeds = numRegisters*8/3;
int fadingMode = 0; //start with all LED's off.

unsigned long startTime = 0; // start time for the chosen fading mode

void setup()   {                
  pinMode(ShiftPWM_latchPin, OUTPUT);
  SPI.setBitOrder(LSBFIRST); // The least significant bit shoult be sent out by the SPI port first.
  // Here you can set the clock speed of the SPI port. Default is DIV4, which is 4MHz with a 16Mhz system clock.
  // If you encounter problems due to long wires or capacitive loads, try lowering the SPI clock.
  SPI.setClockDivider(SPI_CLOCK_DIV4); 
  SPI.begin(); 

  Serial.begin(9600);

  // Sets the number of 8-bit registers that are used.
  ShiftPWM.SetAmountOfRegisters(numRegisters);

  // SetPinGrouping allows flexibility in LED setup. 
  // If your LED's are connected like this: RRRRGGGGBBBBRRRRGGGGBBBB, use SetPinGrouping(4).
  ShiftPWM.SetPinGrouping(1); //This is the default, but I added here to demonstrate how to use the funtion

  ShiftPWM.Start(pwmFrequency,maxBrightness);
  
  
  
  
  
}

void loop() {
  
  // read pulse
  
    valTurn = pulseIn(4,HIGH)*2-700;
    valMove = pulseIn(3,HIGH)*2-700;
    valSw   = pulseIn(2,HIGH)*2-700;
    /*
  Serial.print("VAL:");
  Serial.print(valTurn);
  Serial.print("  ");
  Serial.print(valMove);
  Serial.print("  ");
  Serial.println(valSw);
    */
  // moveRES is result of move
  if(valMove < 1400 ) move = 0;
  if(valMove > 1650) move = 2;
  if((valMove < 1650 ) && (valMove > 1300)) move = 1;
  if (move!=movear[2])
  {
    movear[0] = movear[1];
    movear[1] = movear[2];
    movear[2] = move;
  }
  if((movear[0]==0)&&(movear[1]==1)&&(movear[2]==0)) moveRES = 0;
  else  moveRES = 1;
  if(movear[2]==2) moveRES = 3;
  if((movear[0]==2)&&(movear[1]==1)&&(movear[2]==0)) moveRES = 1;
  if(movear[2]==1) moveRES = 2;  
  
  //calc gaspower
  gasPover = map(valMove,NEUTHROTTE,MAXTHROTTE,0,100);
  
  // read 3d chanel
  if (valSw >1600) valSw = 1;
  else valSw = 0;
  
  // change mode of light
  if (( moveRES == STOP )&&( valSw == 1 ))
  {
    mode++;
    if (mode == 4) mode = 0;
    ShiftPWM.SetAll(255);
    delay(3000);
    ShiftPWM.SetAll(0);
  }
  
  switch(mode)
  {
    case 0:
        
        
    break;
    
    case 1:
    
        ShiftPWM.SetOne(HeadL,255);
        ShiftPWM.SetOne(HeadR,255);
        ShiftPWM.SetOne(GabaritsF,200);
        
        // STOP
        if(moveRES == STOP) 
        {
          ShiftPWM.SetOne(StopL,255);
          ShiftPWM.SetOne(StopR,255);
          ShiftPWM.SetOne(GabaritsB,255);
        }
        else
        {
          ShiftPWM.SetOne(StopL,0);
          ShiftPWM.SetOne(StopR,0);
          ShiftPWM.SetOne(GabaritsB,100);
        }   
        
        // reverse
        if(moveRES == REWERSE) 
        {
          ShiftPWM.SetOne(BackL,255);
          ShiftPWM.SetOne(BackR,255);
        }
        else
        {
          ShiftPWM.SetOne(BackL,0);
          ShiftPWM.SetOne(BackR,0);
        } 
        
        //gas
        if(gasPover>75)
        {
          ShiftPWM.SetOne(Tube,255);
          //strob 
          
          if(millis()-timeStrob>10)
          {            
            timeStrob = millis();
            index1=index1>=sizeof(strobArr1)?0:index1;
            
            ShiftPWM.SetOne(HighL,strobArr1[index1]);
            ShiftPWM.SetOne(HighR,strobArr1[index1]);
            
            index1++; 
          }
        }
        else
        {
          ShiftPWM.SetOne(Tube,0);
          ShiftPWM.SetOne(HighL,0);
          ShiftPWM.SetOne(HighR,0);
          index1 = 0;
        }
        
        //
        if ((valMove>NEUTHROTTE+100)||(valMove<NEUTHROTTE-100))
        {
          timeAlarm = millis();
        } 
                
        if(millis()-timeAlarm>3000)
        {
          
           
          if(millis()-timeAlarm2>10)
          {
            
            timeAlarm2 = millis();
            index2=index2>=sizeof(alarmArr1)?0:index2;
            
            ShiftPWM.SetOne(TurnBR,alarmArr1[index2]);
            ShiftPWM.SetOne(TurnBL,alarmArr1[index2]);
            ShiftPWM.SetOne(TurnFR,alarmArr1[index2]);
            ShiftPWM.SetOne(TurnFL,alarmArr1[index2]);
             
            index2++;
          }
        }
        else
        {
          ShiftPWM.SetOne(TurnBR,0);
          ShiftPWM.SetOne(TurnBL,0);
          ShiftPWM.SetOne(TurnFR,0);
          ShiftPWM.SetOne(TurnFL,0);
          index2 = 0;
        }
        
    break;
  }
  


  
   
  
  
  /*
  Serial.print("MOVE");
  Serial.print(movear[0]);
  Serial.print("  ");
  Serial.print(movear[1]);
  Serial.print("  ");
  Serial.println(movear[2]);
  Serial.print("moveRES");Serial.println(moveRES);
  Serial.print("gasPower");Serial.println(gasPover);
  Serial.println(mode);
  */
  
}



